#ifndef CAROS_UR_AI_PROXY_H_
#define CAROS_UR_AI_PROXY_H_

#include <caros/caros_service_client.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <caros_control_msgs/RobotState.h>
#include <caros_universalrobot/ForceModeStartAction.h>
#include <caros_universalrobot/ForceModeStopAction.h>
#include <caros_universalrobot/ForceModeUpdateAction.h>
#include <caros_universalrobot/MoveLinAction.h>
#include <caros_universalrobot/MovePtpAction.h>
#include <caros_universalrobot/MovePtpPathAction.h>
#include <caros_universalrobot/MoveServoQAction.h>
#include <caros_universalrobot/MoveServoStopAction.h>
#include <caros_universalrobot/MoveVelQAction.h>
#include <caros_universalrobot/MoveVelTAction.h>
#include <caros_universalrobot/SetIOAction.h>
#include <caros_universalrobot/SetPayloadAction.h>
#include <caros_universalrobot/ZeroFTsensorAction.h>
#include <caros_universalrobot/SendCustomScriptFunctionAction.h>
#include <caros_universalrobot/SendCustomScriptFileAction.h>
#include <caros_control_msgs/Status.h>

#include <rw/math.hpp>
#include <rw/trajectory/Path.hpp>
#include <ros/ros.h>
#include <string>

#define USE_THREAD true
#define NO_THREAD false
#define UR_EXECUTION_TIMEOUT 120.0
#define UR_PATH_EXECUTION_TIMEOUT 300.0

namespace caros
{
/**
 * @brief This class implements a C++ proxy to control and read data from a URActionInterface.
 */
class URAIProxy
{
 public:
  /**
   * @brief Constructor
   * @param[in] nodehandle
   * @param[in] devname The name of the CAROS universalrobot node
   */
  URAIProxy(ros::NodeHandle nodehandle, const std::string& devname, const bool blocking = true);

  //! destructor
  virtual ~URAIProxy();

  /**
   * @brief move robot in a linear Cartesian path
   * @param[in] target The target to move to
   * @param[in] speed The tool speed (a value between 0 and 1 is expected) [m/s]
   * @param[in] acceleration Tool acceleration [m/s^2]
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveLin(const rw::math::Transform3D<>& target, const float speed = 0.4f, const float acceleration = 1.2f);

  /**
   * @brief move robot from point to point
   * @param[in] target The target to move to
   * @param[in] speed The joint speed (a value between 0 and 1 is expected) [rad/s]
   * @param[in] acceleration Joint acceleration of leading axis [rad/s^2]
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool movePtp(const rw::math::Q& target, const float speed = 0.4f, const float acceleration = 1.4f);

  /**
   * @brief Move to each joint position specified in a QPath.
   * @param[in] q_path path with joint positions that includes acceleration, speed and blend for each position.
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool movePtpPath(const rw::trajectory::QPath& q_path);

  /**
   * @brief move robot by some implementation specific distance based on the provided joint velocities
   * @param[in] target The joint velocities
   * @param[in] acceleration joint acceleration [rad/s^2] (of leading axis)
   * @param[in] time time [s] before the function returns (optional)
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveVelQ(const rw::math::Q& target, double acceleration, double time = 0.0);

  /**
   * @brief move robot by some implementation specific distance based on the provided velocity screw
   * @param[in] target The velocity screw.
   * @param[in] acceleration joint acceleration [rad/s^2] (of leading axis)
   * @param[in] time time [s] before the function returns (optional)
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveVelT(const rw::math::VelocityScrew6D<>& target, double acceleration, double time = 0.0);

  /**
   * @brief move robot in a servoing fashion using joint configurations
   * @param[in] target The joint configurations to move it
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveServoQ(const rw::math::Q& target, float time, float lookahead_time, float gain);

  /**
   * @brief Stop the servos when servoing
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveServoStop();

  /**
   * @brief Set robot to be controlled in force mode.
   *
   * @general 6D vector = [x, y, z, rx, ry, rz], C = compliant, NC = Non-Compliant
   * @param[in] ref_t_offset Pose vector that defines the force frame relative to the base frame.
   * @param[in] selection 6D vector, setting C in the axis direction (=1) or NC (=0)
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   * @param[in] type An integer [1;3] specifying how the robot interprets the force frame. 1: The force frame is
   * transformed in a way such that its y-axis is aligned with a vector pointing from the robot tcp towards the origin
   *of
   * the force frame. 2: The force frame is not transformed. 3: The force frame is transformed in a way such that its
   * x-axis is the projection of the robot tcp velocity vector onto the x-y plane of the force frame.
   * @param[in] limits 6D vector, C: max tcp speed, NV: max deviation from tcp pose
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveForceModeStart(const rw::math::Transform3D<>& ref_t_offset, const rw::math::Q& selection,
                          const rw::math::Wrench6D<>& wrench_target, int type, const rw::math::Q& limits);

  /**
   * @brief Update the wrench the robot will apply to its environment.
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveForceModeUpdate(const rw::math::Wrench6D<>& wrench_target);

  /**
   * @brief Resets the robot mode from force mode to normal operation.
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool moveForceModeStop();

  /**
   * @brief hard stop the robot
   *
   * @returns a boolean indicating if the UR robot accepted the command
   * @throws UnavailableService when the command is currently unavailable. This indicates that the connection to the
   * UR robot is not fully working, or the UR robot has not announced this service yet.
   * @throws BadServiceCall when an error happened while communicating with the UR robot.
   */
  bool stop();

  /**
   * @brief Send a custom ur script function to the controller
   * @param function_name specify a name for the custom script function
   * @param script the custom ur script to be sent to the controller specified as a string,
   * each line must be terminated with a newline. The code will automatically be indented with one tab
   * to fit with the function body.
   */
  bool sendCustomScriptFunction(const std::string& function_name, const std::string& script);

  /**
   * @brief Send a custom ur script file to the controller
   * @param file_path the file path to the custom ur script file
   */
  bool sendCustomScriptFile(const std::string& file_path);

  /**
   * @brief Zeroes the TCP force/torque measurement from the builtin force/torque sensor by subtracting the current
   * measurement from the subsequent.
   */
  bool zeroFtSensor();

  /**
   * @brief Get the last reported joint configuration
   *
   * @returns the joint configuration (the type of values are very implementation specific, but as a guideline it's
   *highly recommended to keep angles in radians, and distances in meters)
   *
   * @note Make sure to look at getTimeStamp for ensuring the sample is "current enough"
   */
  rw::math::Q getQ();

  /**
   * @brief Get the last reported velocities
   *
   * @returns the velocities (the type of values are implementation specific, but as a guideline it's recommended to
   *represent velocitites as radians per sec)
   *
   * @note Make sure to look at getTimeStamp for ensuring the sample is "current enough"
   */
  rw::math::Q getQd();

  /**
   * @brief Get the last reported TCP force
   *
   * @returns Returns the wrench (Force/Torque vector) at the TCP
   *
   * @note The external wrench is computed based on the error between the joint torques required to stay on the
   * trajectory and the expected joint torques. The maximum force exerted along each axis is 300 Newtons.
   */
  rw::math::Wrench6D<> getTCPForce();

  /**
   * @brief Get the last reported TCP pose
   *
   * @returns Returns the TCP translation and orientation wrt. the robot base
   *
   */
  rw::math::Transform3D<> getTCPPose();

  /**
   * @brief Get information on whether the device's emergency stop is taken
   *
   * @returns a boolean indicating whether the device's emergency stop is taken or not
   *
   */
  bool isEmergencyStopped();

  /**
   * @brief Get information on whether the device's security stop is in effect.
   * @returns a boolean indicating whether the device's security stop is in effect.
   *
   */
  bool isSecurityStopped();

  /**
   * @brief get the timestamp of the received data - use this to verify that the data is "current enough" and supposedly
   *valid - in the case that no data has yet been reported from the device
   *
   * @returns the timestamp of the last reported robot state
   */
  ros::Time getTimeStamp();

 protected:
  void handleRobotState(const caros_control_msgs::RobotState& state);

  ros::NodeHandle nodehandle_;
  bool blocking_;
  std::string ros_namespace_;

  // action client
  actionlib::SimpleActionClient<caros_universalrobot::ForceModeStartAction> ac_move_force_mode_start_;
  actionlib::SimpleActionClient<caros_universalrobot::ForceModeUpdateAction> ac_move_force_mode_update_;
  actionlib::SimpleActionClient<caros_universalrobot::ForceModeStopAction> ac_move_force_mode_stop_;
  actionlib::SimpleActionClient<caros_universalrobot::MoveLinAction> ac_move_lin_;
  actionlib::SimpleActionClient<caros_universalrobot::MovePtpAction> ac_move_ptp_;
  actionlib::SimpleActionClient<caros_universalrobot::MovePtpPathAction> ac_move_ptp_path_;
  actionlib::SimpleActionClient<caros_universalrobot::MoveServoQAction> ac_move_servo_q_;
  actionlib::SimpleActionClient<caros_universalrobot::MoveServoStopAction> ac_move_servo_stop_;
  actionlib::SimpleActionClient<caros_universalrobot::MoveVelQAction> ac_move_vel_q_;
  actionlib::SimpleActionClient<caros_universalrobot::MoveVelTAction> ac_move_vel_t_;
  actionlib::SimpleActionClient<caros_universalrobot::SetIOAction> ac_set_io_;
  actionlib::SimpleActionClient<caros_universalrobot::SetPayloadAction> ac_set_payload_;
  actionlib::SimpleActionClient<caros_universalrobot::ZeroFTsensorAction> ac_zero_ft_sensor_;
  actionlib::SimpleActionClient<caros_universalrobot::SendCustomScriptFunctionAction> ac_send_custom_script_function_;
  actionlib::SimpleActionClient<caros_universalrobot::SendCustomScriptFileAction> ac_send_custom_script_file_;
  // states
  ros::Subscriber sub_robot_state_;
  caros_control_msgs::RobotState robot_state_;
};
}  // namespace caros

#endif  // CAROS_UR_AI_PROXY_H_
